#include "axisgroup/axisgroup_robot.h"
#include "axisgroup/robot_mgr.h"
#include "task/motion_al.h"
#include "rob_common/robot_payload.h"
#include "rob_common/robtool.h"
#include "rob_common/usrframe.h"
#include "log/zuclog.h"
#include "libnml/rcs/rcs.hh"
#include "safety/io/safeio.h"
#include "nml_intf/zuc_nml.hh"

#define TASK_INFO(fmt, ...) zuclog_info("TASK UPDATE", fmt, ##__VA_ARGS__)
#define TASK_ERR(fmt, ...) zuclog_error("TASK UPDATE", fmt, group_id_, ##__VA_ARGS__)
#define TASK_DEBUG(fmt, ...) zuclog_debug("TASK UPDATE", fmt, group_id_, ##__VA_ARGS__)

int zucJointUpdate(int robot_id, ZUC_JOINT_STAT stat[]);
int zucTrajUpdate(int robot_id, ZUC_TRAJ_STAT* stat);

// 兼容之前的motion 后续集中改掉 #TODO
int zucMotionUpdate(ZUC_MOTION_STAT* stat, ZUC_MOTION_STAT* multi_stat)
{
    // 更新机器人状态
    int res = 0;
    for (size_t i = 0; i < mot::al::MotionProxy::instance().robot_cnt(); i++)
    {
        auto rob = mot::al::MotionProxy::instance().robot(i);
        if (!rob.get())
        {
            continue;
        }
        auto motion_robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(i);
        if (!motion_robot.get())
        {
            continue;
        }

        motion_robot->get_robot_status_for_task(&rob->get_status());  // 更新状态

        res = res || zucJointUpdate(i, &(multi_stat[i].joint[0]));
        res = res || zucTrajUpdate(i, &(multi_stat[i].traj));
        mot::al::RobotALIntf::Status cur_status = rob->get_status();

        multi_stat[i].robot_serial_num = cur_status.serial_num;
        multi_stat[i].robot_joint_num = cur_status.robot_joint_num;
        multi_stat[i].powered_on = cur_status.is_power_on;       // 已上电
        multi_stat[i].is_powering_on = cur_status.is_powering;   // 上电中
        multi_stat[i].is_enabled = cur_status.is_enable;         // 已使能
        multi_stat[i].is_being_enable = cur_status.is_enabling;  // 使能中
        multi_stat[i].dhParam = cur_status.robot_dh;             // #TODO 如果是新的dh参数，需要重新设置到TASK相关的运动学库中
        multi_stat[i].status = multi_stat[i].traj.status;        // 运动状态

        // 负载相关参数
        auto config = cur_status.robot_config;
        multi_stat[i].cur_payload_id = config.work_payload_id;  // 机器人负载id
        auto payload = axisgroup::RobotPayloadMgr::get_robot_payload(config.work_payload_id).payload();
        multi_stat[i].payload[0] = payload.m;
        multi_stat[i].payload[1] = payload.p[0];
        multi_stat[i].payload[2] = payload.p[1];
        multi_stat[i].payload[3] = payload.p[2];

        // 坐标系相关参数
        multi_stat[i].currentToolId = config.tool_frame_id;  // 工具坐标系偏移
        multi_stat[i].currentUserId = config.user_frame_id;  // 用户坐标系偏移
        multi_stat[i].base_frame_id = config.base_frame_id;  // 基座标系偏移

        for (int i = 0; i < CART_DIM; i++) { multi_stat[i].drag_dead_zone[i] = cur_status.robot_config.force_control.drag_dead_zone[i]; }

        //安全事件
        multi_stat[i].emergency_stop = safety::SafeIOMgr::instance().is_estop();
        multi_stat[i].protective_stop = safety::SafeIOMgr::instance().is_protective_stop();

        //模型相关参数
        auto& model = multi_stat[i].robot_model_data;
        model.maxMomentum = 12;          //TODO:从配置读取
        model.minMomentum = 0.1;         //TODO:从配置读取
        model.maxRobotPower = 600;       //TODO:从配置读取
        model.minRobotPower = 20;        //TODO:从配置读取
        model.minReduceMomentum = 0;     //TODO:从配置读取
        model.maxReduceMomentum = 0.8;   //TODO:从配置读取
        model.minReduceRobotPower = 0;   //TODO:从配置读取
        model.maxReduceRobotPower = 35;  //TODO:从配置读取
        model.payload = config.robot_model.max_payload;
        model.maxTrajVel = config.robot_model.traj.vel.trans;
        model.maxTrajAcc = config.robot_model.traj.acc.trans;
        config.robot_model.default_pose.initial.array_value(model.defInitalJPos);
        config.robot_model.default_pose.shrunken.array_value(model.defShrinkenPose);
        config.robot_model.default_pose.stretched.array_value(model.defStretchedPose);
        config.robot_model.joint_para.default_joint_max_pos.array_value(model.maxJointPosLimit);
        config.robot_model.joint_para.default_joint_min_pos.array_value(model.minJointPosLimit);
        config.robot_model.joint_para.default_joint_vel_limit.array_value(model.maxJointVel);
        config.robot_model.joint_para.default_joint_acc_limit.array_value(model.maxJointAcc);
        config.robot_model.joint_para.default_joint_err_limit.array_value(model.joint_err_limit);
        model.model = cur_status.serial_num / 10000;

        // 安全姿态相关
        auto& default_pose = cur_status.robot_config.default_pose;
        default_pose.initial.array_value(multi_stat[i].initialPose);
        default_pose.shrunken.array_value(multi_stat[i].shrunkenPose);
        default_pose.stretched.array_value(multi_stat[i].stretchedPose);
        multi_stat[i].initPoseTol = default_pose.tol;
        multi_stat[i].homeVel = default_pose.vel_scale;
    }
    *stat = multi_stat[0];  // 保持原来的接口能获取到robot1的数据
    return 0;

    // res = res || zucTorqSensorUpdate(&stat->sensor);

    // auto robot_motion_task = mot::al::MotionProxy::instance().robot(0); if (!robot_motion_task.get()) {return -1;}
    // int joint;
    // int error;
    // int exec;
    // int dio, aio;
    // unsigned char errtype;
    // int currrent_serial_num = last_serial_num;

    // stat->errAddLine = zucmotStatus.errAddLine;
    // // read the zucmot status

    // stat->on_soft_limit = zucmotStatus.on_soft_limit;
    // stat->next_to_limit = zucmotStatus.next_to_limit;
    // stat->safetyguard_lock = zucmotStatus.safetyguard_lock;
    // stat->category_one_stop_program_pause = zucmotStatus.category_one_stop_program_pause;
    // stat->estop_recovery = zucmotStatus.estop_recovery;
    // stat->is_momentumLimited = zucmotStatus.is_momentumLimited;
    // stat->is_robot_power_limited = zucmotStatus.is_robot_power_limited;
    // stat->is_safezone_limited = zucmotStatus.is_safezone_limited;
    // stat->three_position_enable_limit = zucmotStatus.three_position_enable_limit;
    // stat->is_tcpvel_limited = zucmotStatus.is_tcpvel_limited;
    // stat->percentage_mode_level = zucmotStatus.percentage_mode_level;
    // stat->is_reduce_mode = zucmotStatus.reduce_mode;
    // stat->dragTcpSpeedLimit = zucmotConfig.safetyCfg.dragTcpSpeedLimit;
    // stat->elbow_center_offset = zucmotConfig.safetyCfg.elbow_center_offset;
    // stat->carte_elbow_pos_cmd = zucmotStatus.carte_elbow_pos_cmd;
    // stat->elbow_sphere_radius = zucmotConfig.safetyCfg.elbow_sphere_radius;

    // stat->safetyCRC = zucmotStatus.safetyCRC;
    // stat->maintenance_status = zucmotStatus.maintenance_status;

    // zucmot_joint_status_t* joints;
    // for (int i = 0; i < zucStatus->motion.robot_joint_num; i++)
    // {
    //     stat->is_jointvel_limited[i] = zucmotStatus.is_jointvel_limited[i];
    //     joints = &(zucmotStatus.joint_status[i]);
    //     stat->dragNearLimit[i] = zucmotStatus.dragNearLimit[i];
    //     stat->admit_axis_opt[i] = zucmotStatus.admit_axis_opt[i];
    // }

    // for (joint = 0; joint < zucStatus->motion.robot_joint_num; joint++)
    // {
    //     stat->robot_model_data.motorSupplierId[joint] = zucmotStatus.servo_status.motorSupplier[joint];
    // }

    // set the status flag

    // /*kinematics calculation result*/
    // for (joint = 0; joint < zucStatus->motion.robot_joint_num; ++joint) stat->utilJointPos[joint] = utilJointPos[joint];
    // stat->utilCartPos = utilCartPos;
    // stat->tcpError[0] = tcpCalError[0];
    // stat->tcpError[1] = tcpCalError[1];
    // stat->tcpError[2] = tcpCalError[2];
    // stat->safeatt.conicalCenRpy = conicalCenRpy;
    // stat->safeatt.deviation = deviation;

    // /*model adapting*/

    // if (zucStatus->currSimMode == 1)
    // {
    //     if (zucStatus->currSimRobot > 0)  // else:  currSimRobot <= 0, 表示切换到仿真模式了，但是还没有切换仿真模型，不更新currrent_serial_num，即使用当前模型。
    //     {
    //         currrent_serial_num = zucStatus->currSimRobot;
    //     }
    // }
    // else
    // {
    //     // else:  serial_num_0 <=0, 表示切换到真机模式了，但是还没有上电读取机器人本体型号，不更新currrent_serial_num，即使用当前模型（刚开机则使用usersetting中设置的）。
    //     if (stat->robot_serial_num > 0)
    //     {
    //         currrent_serial_num = stat->robot_serial_num;
    //         zucStatus->currSimRobot = currrent_serial_num;  //切换到真机后真机模型发生变换，仿真模型自动跟随真机
    //     }
    // }
    // if (currrent_serial_num != last_serial_num)
    // {
    //     if (currrent_serial_num <= 0)
    //     {
    //         zucOperatorError(INVALID_ROBOT_SERIAL_NUM, "invalid robot serial number %d", currrent_serial_num);
    //     }
    //     else
    //     {
    //         int old_serial_num = stat->robot_serial_num;
    //         stat->robot_serial_num = currrent_serial_num;

    //         int new_serial_num = stat->robot_serial_num;
    //         if (zucAdapt2Robot(new_serial_num) == 0)
    //         {
    //             int old_series, new_series, old_model, new_model;
    //             old_model = (old_serial_num / 10000);
    //             // old_model = old_serial_num.getProductNumber();  //(old_serial_num/10000);
    //             old_series = old_serial_num / 100000000;  // (old_serial_num/100000000);
    //             new_model = stat->robot_serial_num / 10000;
    //             // new_model = new_serial_num.getProductNumber();  // (stat->robot_serial_num/10000);
    //             new_series = stat->robot_serial_num / 100000000;  // (stat->robot_serial_num/100000000);
    //             rcs_print("\nrobot model adaption succeeded: %d, %d\n", old_model, new_model);
    //             // series changed between Zu and MC
    //             if (new_model != old_model)
    //             {
    //                 rcs_print("\nrobot model custom pose changed: %d, %d\n", old_model, new_model);
    //                 zucSetCustomPose(0, stat->robot_model_data.defShrinkenPose, 1);
    //                 zucSetCustomPose(1, stat->robot_model_data.defStretchedPose, 1);
    //                 zucSetCustomPose(2, stat->robot_model_data.defInitalJPos, 1);
    //             }
    //             iniSaveSerialNum(stat->robot_serial_num, zuc_inifile);
    //         }
    //     }
    // }
    // last_serial_num = currrent_serial_num;

    // /*payload identification*/
    // if (stat->identifyTrajIndex >= 0)
    // {
    //     if (stat->identifyWithLoad)
    //     {
    //         stat->onloadIdentifyStat[stat->identifyTrajIndex] = zucmotStatus.identifying_status;
    //     }
    //     else
    //     {
    //         stat->noloadIdentifyStat[stat->identifyTrajIndex] = zucmotStatus.identifying_status;
    //     }
    // }
    // if (last_identifying_status != 2 && zucmotStatus.identifying_status == 2)
    // {
    //     stat->identifyTrajIndex = -1;
    //     resetPayloadIdentifyStat();
    //     rcs_print("identifying complete\n");
    // }
    // else if (last_identifying_status != -1 && zucmotStatus.identifying_status == -1)
    // {
    //     stat->identifyTrajIndex = -1;
    //     resetPayloadIdentifyStat();
    //     rcs_print("identifying error\n");
    // }
    // last_identifying_status = zucmotStatus.identifying_status;

    // stat->percentage_mode_level1_val = zucmotStatus.percentage_mode_level1_val;
    // stat->percentage_mode_level2_val = zucmotStatus.percentage_mode_level2_val;
    // // stat->identifyfrictionStat = zucmotStatus.friction_identifying_status;
    // // for (int i = 0; i < ZUCMOT_MAX_JOINTS * ZUCMOT_MAX_JOINTS; i++) { stat->identifyfrictionstartposStat[i] = zucmotStatus.identify_friction_start_pos[i]; }
    // // for (int i = 0; i < ZUCMOT_MAX_JOINTS * ZUCMOT_MAX_JOINTS; i++) { stat->identifyfrictionendposStat[i] = zucmotStatus.identify_friction_end_pos[i]; }
    // // for (int i = 0; i < zucStatus->motion.robot_joint_num; i++) { stat->jointcollisionsensitivityStat[i] = zucmotStatus.joint_sensitivity[i]; }
    // // for (int i = 0; i < zucStatus->motion.robot_joint_num; i++) { stat->jointfrictioncompensateStat[i] = zucmotStatus.friction_compensate[i]; }
    // // stat->dynamicsIdentifyStat = zucmotStatus.dynamics_identifying_status;  // update state
    // // for (int i = 0; i < ELE_IDENTI_TRAJ_DIMENSION; i++)
    // // {
    // //     stat->dynamicsIdentifyTrajPara[i] = zucStatus->motion.robot_model_data.dynamics_identify_traj_para[i];
    // // }
    // // for (int i = 0; i < zucStatus->motion.robot_joint_num; i++) { stat->dynamicsIdentifyStartPos[i] = zucmotStatus.dynamics_identify_start_pos[i]; }

    // // if (last_dynamics_identify_status == 1 && zucmotStatus.dynamics_identifying_status == 2)
    // // {
    // //     zucGetDynamicsIdentifyResult();
    // // }
    // // last_dynamics_identify_status = zucmotStatus.dynamics_identifying_status;

    // // if (last_friction_identify_status == 1 && zucmotStatus.friction_identifying_status == 2)
    // // {
    // //     zucGetFrictionIdentifyResult();
    // // }
    // // last_friction_identify_status = zucmotStatus.friction_identifying_status;

    // if (dynamics_identify_pid != -1)
    // {
    //     int status = 0;
    //     pid_t ret = waitpid(dynamics_identify_pid, &status, WNOHANG);  // wait for child process to exit, in case of zombee process
    //     if (ret > 0)
    //     {
    //         printf("wait child success...\n");
    //         printf("exit code:%d\n", WEXITSTATUS(status));
    //         dynamics_identify_pid = -1;
    //         stat->dynamicsIdentifyStat = 4;
    //         zucCompleteDynamicsIdentify();
    //     }
    //     else if (ret == -1)
    //     {
    //         printf("waitpid error...\n");
    //     }
    // }

    // if (friction_identify_pid != -1)
    // {
    //     int status = 0;
    //     pid_t ret = waitpid(friction_identify_pid, &status, WNOHANG);  // wait for child process to exit, in case of zombee process
    //     if (ret > 0)
    //     {
    //         printf("wait child success...\n");
    //         printf("exit code:%d\n", WEXITSTATUS(status));
    //         friction_identify_pid = -1;
    //         stat->identifyfrictionStat = 4;
    //         zucCompleteFrictionIdentify();
    //     }
    //     else if (ret == -1)
    //     {
    //         printf("waitpid error...\n");
    //     }
    // }

    // stat->backdriveStat = zucmotStatus.backdrive_state;
    // stat->forcelimitvalueStat = zucmotStatus.force_limit_value;
    // stat->collisiondetectionenableStat = zucmotStatus.collision_detection_enable;

    // stat->steppingWait = zucmotDebug.stepping;

    // stat->torque_control_enable = zucmotStatus.torque_control_settings.torque_control_enable;
    // for (int i = 0; i < 3; i++) { stat->gravity_rpy[i] = zucmotStatus.gravity_rpy[i]; }

    // // 同步Motion层的状态
    // stat->compliantType = zucmotStatus.compliantType;
    // stat->robot_real_power = zucmotStatus.robot_power;
    // stat->robot_real_momentum = zucmotStatus.robot_momentum;

    // for (int i = 0; i < 8; i++) { stat->captureStatus[i] = zucmotStatus.captureStatus[i]; }

    // if (servo_upgrade->ErrorCode)
    // {
    //     zucOperatorError(SERVO_FIRMWARE_UPGRADE_ERROR, "failed to upgrade servo firmware: 0x%x\n", servo_upgrade->ErrorCode);
    //     servo_upgrade->ErrorCode = 0;
    // }

    return (res == 0) ? 0 : -1;
}

int zucJointUpdate(int robot_id, ZUC_JOINT_STAT stat[])
{
    // 更新关节对外信息

    auto rob = mot::al::MotionProxy::instance().robot(robot_id);
    if (!rob.get())
    {
        return -1;
    }

    auto status = rob->get_status();
    auto config = status.robot_config;
    for (int joint_num = 0; joint_num < status.robot_joint_num; joint_num++)
    {
        stat[joint_num].output = status.cmd_servo_jpos[joint_num];  // 伺服指令位置
        stat[joint_num].input = status.act_jpos[joint_num];         // 伺服反馈位置

        stat[joint_num].enabled = status.is_enable;
        stat[joint_num].inpos = status.is_act_inpos;
        stat[joint_num].cmd_tor = status.cmd_tor[joint_num];
        stat[joint_num].act_tor = status.act_tor[joint_num];

        //关节安全参数
        stat[joint_num].minPositionLimit = config.joint_limit.min[joint_num];
        stat[joint_num].maxPositionLimit = config.joint_limit.max[joint_num];
        stat[joint_num].maxVel = config.joint_limit.vel[joint_num];
        stat[joint_num].maxAcc = config.joint_limit.acc[joint_num];
        stat[joint_num].err_limit = config.joint_limit.err_limit[joint_num];
    }

    int joint_num;
    zucmot_joint_status_t* joint;
#ifdef WATCH_FLAGS
    static int old_joint_flag[8];
#endif

    //更新关节统计信息
    // ServoStatistic::get()->updateDaliy();
    // if (zucmotStatus.powered_on)
    // {
    //     for (int i = 0; i < numJoints; i++)
    //     {
    //         joint = &(zucmotStatus.joint_status[i]);
    //         ServoStatistic::get()->update(i, 0x8384, joint->servo_warn_8384_cnt);
    //         ServoStatistic::get()->update(i, 0x8385, joint->servo_warn_8385_cnt);
    //         ServoStatistic::get()->update(i, 0x8387, joint->servo_warn_8387_cnt);
    //         ServoStatistic::get()->update(i, 0x8388, joint->servo_warn_8388_cnt);
    //         ServoStatistic::get()->update(i, 0x7001, joint->servo_warn_7001_cnt);
    //         stat[i].servo_warn_8384_cnt = ServoStatistic::get()->getAllCnt(i, 0x8384);
    //         stat[i].servo_warn_8385_cnt = ServoStatistic::get()->getAllCnt(i, 0x8385);
    //         stat[i].servo_warn_8387_cnt = ServoStatistic::get()->getAllCnt(i, 0x8387);
    //         stat[i].servo_warn_8388_cnt = ServoStatistic::get()->getAllCnt(i, 0x8388);
    //         stat[i].servo_warn_7001_cnt = ServoStatistic::get()->getAllCnt(i, 0x7001);
    //         stat[i].servo_warn_8384_daliy_cnt = ServoStatistic::get()->getDaliyCnt(i, 0x8384);
    //         stat[i].servo_warn_8385_daliy_cnt = ServoStatistic::get()->getDaliyCnt(i, 0x8385);
    //         stat[i].servo_warn_8387_daliy_cnt = ServoStatistic::get()->getDaliyCnt(i, 0x8387);
    //         stat[i].servo_warn_8388_daliy_cnt = ServoStatistic::get()->getDaliyCnt(i, 0x8388);
    //         stat[i].servo_warn_7001_daliy_cnt = ServoStatistic::get()->getDaliyCnt(i, 0x7001);
    //     }
    //     int statErr = ServoStatistic::get()->check(false);
    //     if (statErr)
    //     {
    //         ServoStatistic::get()->printServoStatistic();
    //         zucOperatorWarning(
    //             SERVO_ENCODE_ERROR, "joint encode statistic warning#{\"fixed_key\":[\"%s\"]}", ServoStatistic::get()->formatDaliyWarnStr().c_str());
    //     }
    // }
    return 0;
}

int zucTrajUpdate(int robot_id, ZUC_TRAJ_STAT* stat)
{
    auto rob = mot::al::MotionProxy::instance().robot(robot_id);
    if (!rob.get())
    {
        return -1;
    }

    auto status = rob->get_status();
    stat->joints = status.robot_joint_num;
    stat->enabled = status.is_enable;

    stat->position_desired = status.desired_tcp_pose;
    stat->position = status.cmd_tcp_pose;
    stat->actualPosition = status.act_tcp_pose;

    if (status.is_error)
    {
        stat->status = RCS_STATUS::RCS_ERROR;
    }
    else if (status.is_act_inpos && (stat->queue == 0))  // #TODO  LW
    {
        stat->status = RCS_STATUS::RCS_DONE;
    }
    else
    {
        stat->status = RCS_STATUS::RCS_EXEC;
    }
    stat->inpos = status.is_act_inpos;       // 规划停止，反馈到位
    stat->rapid_scale = status.rapid_scale;  // coord程序运行的倍率
    stat->paused = status.is_paused;         // motion 层暂停
    // stat->velocity = zucmotStatus.vel;
    // stat->acceleration = zucmotStatus.acc;

    // int joint, enables;
    // stat->deprecated_axes = TrajConfig.DeprecatedAxes;
    // stat->axis_mask = TrajConfig.AxisMask;
    // stat->linearUnits = TrajConfig.LinearUnits;
    // stat->angularUnits = TrajConfig.AngularUnits;

    /*******************************************
     * task最好能不涉及motion规划模式，后续更改 #TODO
     * *****************************************/
    // if (zucmotStatus.motionFlag & ZUCMOT_MOTION_DRAG_BIT)
    //     stat->mode = ZUC_TRAJ_MODE_DRAG;
    // else if (zucmotStatus.motionFlag & ZUCMOT_MOTION_TELEOP_BIT)
    //     stat->mode = ZUC_TRAJ_MODE_TELEOP;
    // else if (zucmotStatus.motionFlag & ZUCMOT_MOTION_TELEOP_TOOL_BIT)
    //     stat->mode = ZUC_TRAJ_MODE_TELEOP_TOOL;
    // else if (zucmotStatus.motionFlag & ZUCMOT_MOTION_SERVOJOP_BIT)
    //     stat->mode = ZUC_TRAJ_MODE_SERVOJOP;
    // else if (zucmotStatus.motionFlag & ZUCMOT_MOTION_ADMITTANCE_BIT)
    //     stat->mode = ZUC_TRAJ_MODE_ADMITTANCE;
    // else
    //     stat->mode = zucmotStatus.motionFlag & ZUCMOT_MOTION_COORD_BIT ? ZUC_TRAJ_MODE_COORD : ZUC_TRAJ_MODE_FREE;

    /* enabled if motion enabled and all joints enabled */
    // static int lastmode = 0;
    // if (lastmode != usrmotGetEmergencyStopPaused())
    // {
    //     printf("####################################################zucTraijUpdate()   stat->mode = ZUC_TRAJ_MODE_COORD  / %d \n", stat->mode);
    // }
    // lastmode = usrmotGetEmergencyStopPaused();
    // if (usrmotGetEmergencyStopPaused() == 1)
    // {
    //     stat->mode = ZUC_TRAJ_MODE_COORD;
    // }

    // stat->queue = zucmotStatus.depth;
    // stat->activeQueue = zucmotStatus.activeDepth;
    // stat->queueFull = zucmotStatus.queueFull;
    // stat->id = zucmotStatus.id;
    // stat->motion_type = zucmotStatus.motionType;
    // stat->distance_to_go = zucmotStatus.distance_to_go;
    // stat->dtg = zucmotStatus.dtg;
    // stat->current_vel = zucmotStatus.current_vel;
    // stat->curr_tcp_trans_vel = zucmotStatus.curr_tcp_trans_vel;
    // stat->curr_tcp_angular_vel = zucmotStatus.curr_tcp_angular_vel;
    // memcpy(stat->curr_tcp_rotation_axis, zucmotStatus.curr_tcp_rotation_axis, 3 * sizeof(double));
    // stat->curr_elbow_vel = zucmotStatus.curr_elbow_vel;
    // stat->vel_tcp_angular = zucmotStatus.vel_tcp_angular;
    // stat->acc_tcp_linear = zucmotStatus.acc_tcp_linear;
    // stat->acc_tcp_angular = zucmotStatus.acc_tcp_angular;
    // if (ZUC_DEBUG_MOTION_TIME & zuc_debug)
    // {
    //     if (stat->id != last_id)
    //     {
    //         if (last_id != last_id_printed)
    //         {
    //             rcs_print("Motion id %d took %f seconds.\n", last_id, etime() - last_id_time);
    //             last_id_printed = last_id;
    //         }
    //         last_id = stat->id;
    //         last_id_time = etime();
    //     }
    // }
    // stat->scale = zucmotStatus.feed_scale;
    // stat->curSegTargetPose = zucmotStatus.curSegTargetPose;

    // stat->maxAcceleration = TrajConfig.MaxAccel;
    // for (int i = 0; i < TrajConfig.Joints; i++) { stat->goalJointPos[i] = zucmotStatus.goalJointPos[i]; }
    // if (zucmotStatus.motionFlag & ZUCMOT_MOTION_COORD_BIT)
    //     enables = zucmotStatus.enables_queued;
    // else
    //     enables = zucmotStatus.enables_new;
    // stat->feed_override_enabled = enables & FS_ENABLED;

    // if (ZUC_DEBUG_MOTION_TIME & zuc_debug)
    // {
    //     if (stat->status == RCS_DONE && last_status != RCS_DONE && stat->id != last_id_printed)
    //     {
    //         rcs_print("Motion id %d took %f seconds.\n", last_id, etime() - last_id_time);
    //         last_id_printed = last_id = stat->id;
    //         last_id_time = etime();
    //     }
    // }

    // // stat->compliantType = zucmotStatus.compliantType;

    // stat->admittance_enabled = zucmotStatus.admittance_enabled;

    // if (new_config)
    // {
    //     stat->cycleTime = zucmotConfig.trajCycleTime;
    //     stat->kinematics_type = zucmotConfig.kinType;
    //     stat->maxVelocity = zucmotConfig.limitVel;
    // }

    // stat->servoMtnFltParam.filterType = TrajConfig.filterType;
    // stat->servoMtnFltParam.mmf_buf_size = TrajConfig.mmf_buf_size;
    // stat->servoMtnFltParam.mmf_ka = TrajConfig.mmf_ka;
    // stat->servoMtnFltParam.mmf_kv = TrajConfig.mmf_kv;
    // stat->servoMtnFltParam.mmf_kp = TrajConfig.mmf_kp;
    // stat->servoMtnFltParam.nlf_vp = TrajConfig.nlf_vp;
    // stat->servoMtnFltParam.nlf_ap = TrajConfig.nlf_ap;
    // stat->servoMtnFltParam.nlf_jp = TrajConfig.nlf_jp;
    // stat->servoMtnFltParam.nlf_vr = TrajConfig.nlf_vr;
    // stat->servoMtnFltParam.nlf_ar = TrajConfig.nlf_ar;
    // stat->servoMtnFltParam.nlf_jr = TrajConfig.nlf_jr;
    // stat->servoMtnFltParam.lpf_cutoff_freq = TrajConfig.lpf_cutoff_freq;

    return 0;
}

int zucUpdate(ZUC_STAT* stat)
{
    for (uint32_t i = 0; i < axisgroup::RobotToolMgr::MAX_ROBOT_TOOL_NUM; i++) { stat->toolOffset[i] = axisgroup::RobotToolMgr::get_rob_tool(i).pose(); }
    for (uint32_t i = 0; i < axisgroup::UsrFrameMgr::MAX_USR_FRAME_NUM; i++) { stat->userOffset[i] = axisgroup::UsrFrameMgr::get_usr_frame(i).pose(); }
    for (uint32_t i = 0; i < axisgroup::RobotPayloadMgr::MAX_ROBOT_PAYLOAD_NUM; i++)
    {
        auto payload = axisgroup::RobotPayloadMgr::get_robot_payload(i).payload();
        stat->payload[i].m = payload.m;
        stat->payload[i].p[0] = payload.p[0];
        stat->payload[i].p[1] = payload.p[1];
        stat->payload[i].p[2] = payload.p[2];
    }
    return 0;
}